import matplotlib.pyplot as plt
from shapely.geometry import Polygon, LineString
from pyproj import Proj, transform
import math
import csv
# 输入数据
lat_list = [30.51935833, 30.5179598, 30.51761963,30.51823611]
lon_list = [120.72055278, 120.7189677, 120.72053888,120.72221312]

# 坐标转换
proj_latlon = Proj(proj="latlong", datum="WGS84")
proj_utm = Proj(proj="utm", zone=50, datum="WGS84")  # 示例为杭州附近Zone 50
x_list, y_list = transform(proj_latlon, proj_utm, lon_list, lat_list)
print(x_list,y_list)
polygon = list(zip(x_list, y_list))#按位置组合
print(polygon)

# 创建多边形
poly = Polygon(polygon)

# 相机参数
fov = math.radians(75)
H = 8
coverage_width = 2 * H * math.tan(fov / 2)
step = 0.9 * coverage_width

# 生成扫描线
lines = []
minx, miny, maxx, maxy = poly.bounds
x = minx
while x < maxx:
    line = LineString([(x, miny - 10), (x, maxy + 10)])
    clipped = line.intersection(poly)
    print("clipped=",clipped)
    if not clipped.is_empty:
        lines.append(clipped)
        print(lines)
    x += step

# 提取路径点
waypoints = []
for i, segment in enumerate(lines):
    if segment.geom_type == 'MultiLineString':
        for line in segment:
            pts = list(line.coords)
            if i % 2 == 1:
                pts.reverse()
            waypoints.extend(pts)
    elif segment.geom_type == 'LineString':
        pts = list(segment.coords)
        if i % 2 == 1:
            pts.reverse()
        waypoints.extend(pts)

# 绘图
fig, ax = plt.subplots(figsize=(8, 8))
# 多边形区域
x_poly, y_poly = poly.exterior.xy
ax.plot(x_poly, y_poly, 'k-', label='Scan Area')

# scan line
for seg in lines:
    if seg.geom_type == 'LineString':
        x_line, y_line = seg.xy
        ax.plot(x_line, y_line, 'orange', linewidth=1)
    elif seg.geom_type == 'MultiLineString':
        for l in seg:
            x_line, y_line = l.xy
            ax.plot(x_line, y_line, 'orange', linewidth=1)

# 航点路径
x_wp, y_wp = zip(*waypoints)
ax.plot(x_wp, y_wp, 'b.-', label='Waypoint Path')

print("航点：",x_wp,y_wp)#UTM
lon_list_out, lat_list_out = transform(proj_utm, proj_latlon, x_wp, y_wp)
print("经纬度：",list(zip(lat_list_out,lon_list_out)))
#建立csv文件
waypoints = list(zip(lat_list_out, lon_list_out))
with open("waypoints.csv", "w", newline="") as f:
    writer = csv.writer(f)
    writer.writerow(["lat", "lon"])
    writer.writerows(waypoints)


#打印显示
# ax.set_title('Drone Coverage Path Planning')
# ax.set_aspect('equal')
# ax.legend()
# plt.xlabel("X (meters)")
# plt.ylabel("Y (meters)")
# plt.grid(True)
# plt.show()
